#include "PowerSpectrum.h"
#include "Int.h"
#include "/home/mjarvis/misc/nr/ran0.cpp"
#include "/home/mjarvis/misc/nr/gasdev.cpp"

#include <fstream>
using std::ifstream;
using std::ofstream;
#include <iostream>
#include <sstream>
using std::istringstream;
#include <string>
using std::string;
#include <map>
using std::map;
#include <vector>
using std::vector;

#include "TMV.h"
using tmv::Vector;
using tmv::Matrix;
#include "TMV_Sym.h"
using tmv::SymMatrix;
#include "dbg.h"
using std::endl;
using std::abs;

#define DATECODE "0622"

// Add NAME to the output names (lnlike.NAME.dat and predlike.NAME.debug)

//#define SN
//#define BO

#define DES
#define LAMBDA
//#define NonFlat
//#define Wa

#ifdef SN
#define NAME "sn"
#elif defined BO
#define NAME "bo"
#else
#define NAME "lens"
#endif

//#define GT4  
// Only use R > 4 for map^2 data
//#define DILLUT  1.10
// Assume dillution error, so multiply each gamma by DILLUT
//#define BMODE -1.0
// +1.0 for add B mode, -1.0 for subract
//#define PD
// Use Peacock-Dodds

#define BURNIN 200
#define MINPERCHAIN 2000
//#define BURNIN 200
//#define MINPERCHAIN 10000
#define NCHAIN 4
#define TESTFREQ 200
#define MAXR 1.1
#define MAXESIGMA 2.0
#define MINESIGMA 0.001

//#define TAKEEVERYN 10

std::ostream* dbgout=0;
bool XDEBUG=false;
//bool XDEBUG=true;

const double PI = 3.141592653589793;
const double DEG = PI/180.;
const double ARCSEC = DEG/3600.;
const double ARCMIN = DEG/60.;

const double map_minr = 10.0*ARCMIN;
const double map_maxr = 150.0*ARCMIN;
const double var_minr = 100.0*ARCMIN;
const double var_maxr = 300.0*ARCMIN;
const double singthresh = 1.e-7;
const int datafrac = 1; // take every nth line of the data
const int Ndelta = 2;

class MCPos;

extern MCPos initbest;
extern MCPos initsigma;
extern MCPos maxdelta;

#ifdef DILLUT
const double DILLUTSQ = DILLUT*DILLUT;
#endif

#ifdef SN
extern "C" {
    void compute_sn1a_chi2_(
        const double& Om_m, const double& Om_de,
        const double& w, const double& wa, double& chisq);
}
#else
void ReadData(
    Vector<double>*& r, Vector<double>*& mean, SymMatrix<double>*& cov, 
    size_t& nmapout);
#endif // SN

double range_rand(double min, double max)
{ return min + (max-min)*ran0(); }

double gauss_rand(double mu, double sigma)
{ return gasdev()*sigma + mu; }

#ifdef DES
double lnomch2_min = -5.0;
double lnomch2_max = 0.0;
double initlnomch2_min = -3.0;
double initlnomch2_max = -1.0;
double lnAs_min = -5.0;
double lnAs_max = 2.0;
double initlnAs_min = -2.0;
double initlnAs_max = 1.0;
// The rest are fixed with Om_b = 0.04, h = 0.7, n = 1, w = -1, wa = 0
double ombh2_min = 0.0196;
double ombh2_max = 0.0196;
double initombh2_min = 0.0196;
double initombh2_max = 0.0196;
double h_min = 0.7;
double h_max = 0.7;
double inith_min = 0.7;
double inith_max = 0.7;
double n_min = 1.0;
double n_max = 1.0;
double initn_min = 1.0;
double initn_max = 1.0;
double w_min = -1.0;
double w_max = -1.0;
double initw_min = -1.0;
double initw_max = -1.0;
double wa_min = 0.0;
double wa_max = 0.0;
double initwa_min = 0.0;
double initwa_max = 0.0;
#else
double ombh2_min = 0.02;
double ombh2_max = 0.025;
double initombh2_min = 0.02;
double initombh2_max = 0.025;
double lnomch2_min = -5.0;
double lnomch2_max = 0.0;
double initlnomch2_min = -3.0;
double initlnomch2_max = -0.5;
#ifdef NonFlat
double omde_min = 0.0;
double omde_max = 2.0;
double initomde_min = 0.0;
double initomde_max = 1.0;
#endif
#ifdef Wa
double w_min = -6.;
double w_max = 4.;
double initw_min = -2.;
double initw_max = -0.5;
#elif defined(LAMBDA)
double w_min = -1.0;
double w_max = -1.0;
double initw_min = -1.;
double initw_max = -1.;
#else
double w_min = -3.2;
double w_max = 0.0;
double initw_min = -2.;
double initw_max = -0.5;
#endif
double wa_min = -6.;
double wa_max = 8.;
double initwa_min = -1.;
double initwa_max = 1.;
double h_min = 0.4;
double h_max = 0.9;
double inith_min = 0.4;
double inith_max = 0.9;
#ifdef LAMBDA
double n_min = 0.5;
double n_max = 1.5;
double initn_min = 0.9;
double initn_max = 1.2;
double lnAs_min = -5.0;
double lnAs_max = 1.5;
double initlnAs_min = -1.0;
double initlnAs_max = 1.;
#else
double n_min = 0.9;
double n_max = 1.1;
double initn_min = 0.9;
double initn_max = 1.1;
double lnAs_min = -1.0;
double lnAs_max = 1.0;
double initlnAs_min = -1.0;
double initlnAs_max = 1.0;
#endif
#endif // DES

#ifdef NonFlat

#ifdef Wa
const size_t NParams = 8;
#else
#ifdef LAMBDA
const size_t NParams = 6;
#else
const size_t NParams = 5;
#endif
#endif
const size_t iw = 6;
const size_t iwa = 7;

#else // !NonFlat

#ifdef Wa
const size_t NParams = 7;
#else
#ifdef LAMBDA
const size_t NParams = 5;
#else
const size_t NParams = 6;
#endif
#endif
const size_t iw = 5;
const size_t iwa = 6;

#endif // NonFlat

const bool fixed_params[NParams] = 
#ifdef DES
{ true, false, true, true, false };
#else
{ false, false, false, false, false };
#endif

const bool includeincovarfit[NParams] = 

#ifdef DES

{ false, true, false, false, true };

#else

#ifdef SN

#ifdef NonFlat

#ifdef Wa
// omb   omc    h     n     As    Om_de  w    wa
{ true, true, true, false, false, true, true, true };
#else
#ifdef LAMBDA
{ true, true, true, false, false, true };
#else
{ true, true, true, false, false, true, true };
#endif // LAMBDA
#endif // Wa

#else // !NonFlat

#ifdef Wa
// omb   omc    h     n     As      w    wa
{ true, true, true, false, false, true, true };
#else
#ifdef LAMBDA
{ true, true, true, false, false };
#else
{ true, true, true, false, false, true };
#endif // LAMBDA
#endif // Wa

#endif // NonFlat

#elif defined BO

#ifdef NonFlat

#ifdef Wa
// omb   omc    h       n    As   Om_de  w     wa
{ true, true, true, false, false, true, true, true };
#else
#ifdef LAMBDA
{ true, true, true, false, false, true };
#else
{ true, true, true, false, false, true, true };
#endif // LAMBDA
#endif // Wa

#else // !NonFlat

#ifdef Wa
// omb   omc    h       n    As    w     wa
{ true, true, true, false, false, true, true };
#else
#ifdef LAMBDA
{ true, true, true, false, false };
#else
{ true, true, true, false, false, true };
#endif // LAMBDA
#endif // Wa

#endif // NonFlat

#else // Lensing

#ifdef NonFlat

#ifdef Wa
// omb   omc    h       n    As   Om_de  w     wa
{ false, true, false, true, true, true, true, true };
#else
#ifdef LAMBDA
{ false, true, false, true, true, true };
#else
{ false, true, false, true, true, true, true };
#endif // LAMBDA
#endif // Wa

#else // !NonFlat

#ifdef Wa
// omb   omc    h       n    As    w     wa
{ false, true, false, true, true, true, true };
#else
#ifdef LAMBDA
{ false, true, false, true, true };
#else
{ false, true, false, true, true, true };
#endif // LAMBDA
#endif // Wa

#endif // NonFlat

#endif // SN

#endif // DES

template <class T> T SQR(T x) { return x*x; }

class MCPos {

private :

    Vector<double> params;

    static SymMatrix<double>* covar;
    static Vector<double> eigensigmas;
    static MCPos* upos; // which MPos is current u set for

    explicit MCPos(int) : params(NParams) {}

public :

    static Universe* u;

    MCPos() : params(NParams)
    {
        bool neghsq;
        double omm;
        do {
#ifdef NonFlat
            do {
                params(0) = range_rand(initombh2_min,initombh2_max);
                params(1) = range_rand(initlnomch2_min,initlnomch2_max);
                params(2) = range_rand(inith_min,inith_max);
                params(5) = range_rand(initomde_min,initomde_max);
                omm = (ombh2()+omch2())/(h()*h());
            } while (omm+omde() >= 1.0);
#else
            do {
                params(0) = range_rand(initombh2_min,initombh2_max);
                params(1) = range_rand(initlnomch2_min,initlnomch2_max);
                params(2) = range_rand(inith_min,inith_max);
                omm = (ombh2()+omch2())/(h()*h());
            } while (omm >= 1.0);
#endif // NonFlat
#ifndef LAMBDA
            params(iw) = range_rand(initw_min,initw_max);
#endif
#ifdef Wa
            params(iwa) = range_rand(initwa_min,initwa_max);
#endif
            neghsq = NegHsq(omm,omde(),w(),wa());
            if (neghsq) {
                dbg<<"NegHsq for "<<params<<endl;
            }
        } while (neghsq);
        params(3) = range_rand(initn_min,initn_max);
        params(4) = range_rand(initlnAs_min,initlnAs_max);
    }

    MCPos(double ombh2, double lnomch2, double h, double n, 
          double lnAs, 
#ifdef NonFlat
          double omde,
#endif
          double w=-1., double wa=0.) : params(NParams)
    { 
        params(0) = ombh2;
        params(1) = lnomch2;
        params(2) = h;
        params(3) = n;
        params(4) = lnAs;
#ifdef NonFlat
        params(5) = omde;
#endif
#ifndef LAMBDA
        params(iw) = w;
#endif
#ifdef Wa
        params(iwa) = wa;
#endif
    }

    MCPos(const MCPos& rhs) : params(rhs.params) {}

    ~MCPos() { }

    void SetUpU() const
    {
        if (upos != this) {
            xdbg<<"Start setupu"<<endl;
            if (u) delete u;
#ifdef NonFlat
            UParamsNonFlat up(ombh2(),omch2(),omde(),n(),w(),wa(),As(),h());
#else
            UParams up(ombh2(),omch2(),n(),w(),wa(),As(),h());
#endif
            u = new Universe(up);
            xdbg<<"Done setupu"<<endl;
        }
    }

    void ResetU() const
    { 
        if (upos == this) {
            if (u) delete u;
            u = 0;
        }
    }

    double ombh2() const { return params(0); }
    double omch2() const { return exp(params(1)); }
    double lnomch2() const { return params(1); }
    double h() const { return params(2); }
    double n() const { return params(3); }
    double As() const { return exp(params(4)); }
    double lnAs() const { return params(4); }
#ifdef NonFlat
    double omde() const { return params(5); }
#else
    double omde() const { return 1.-GetOmM(); }
#endif
#ifdef LAMBDA
    double w() const { return -1.; }
#else
    double w() const { return params(iw); }
#endif
#ifdef Wa
    double wa() const { return params(iwa); }
#else
    double wa() const { return 0.; }
#endif
    double GetOmM() const { return (ombh2()+omch2())/h()/h(); }
    double GetOmB() const { return ombh2()/h()/h(); }
    double GetOmDE() const {
#ifdef NonFlat
        return omde();
#else
        return 1.-GetOmM();
#endif
    }

    double Sigma8() const 
    {
        SetUpU();
        return u->sigma8();
    }

    const Vector<double>& GetParams() const { return params; }

    bool InRange() const
    { 
        //xdbg<<"inrange?"<<endl;
        if (ombh2() < ombh2_min || ombh2() > ombh2_max) {
            xdbg<<"ombh2 = "<<ombh2()<<" not in range\n";
            return false;
        }
        if (lnomch2() < lnomch2_min || lnomch2() > lnomch2_max) {
            xdbg<<"omch2 = "<<omch2()<<" not in range\n";
            return false;
        }
        if (h() < h_min || h() > h_max) {
            xdbg<<"h = "<<h()<<" not in range\n";
            return false;
        }
#ifdef NonFlat
        if (omde() < omde_min || omde() > omde_max) {
            xdbg<<"omde = "<<omde()<<" not in range\n";
            return false;
        }
#endif
#ifndef LAMBDA
        if (w() < w_min || w() > w_max) {
            xdbg<<"w = "<<w()<<" not in range\n";
            return false;
        }
#endif
        if (n() < n_min || n() > n_max) {
            xdbg<<"n = "<<n()<<" not in range\n";
            return false;
        }
        if (lnAs() < lnAs_min || lnAs() > lnAs_max) {
            xdbg<<"As = "<<As()<<" not in range\n";
            return false;
        }
#ifdef Wa
        if (wa() < wa_min || wa() > wa_max) {
            xdbg<<"wa = "<<wa()<<" not in range\n";
            return false;
        }
#endif
        if (GetOmM() >= 1.0) {
            xdbg<<"Om_m = "<<GetOmM()<<" not in range\n";
            return false;
        }
        //xdbg<<"yep."<<endl;
        return true;
    }

    MCPos* Step() const 
    { 
        xdbg<<"start step"<<endl;

        MCPos* newp = new MCPos(1);
        do {
            for(size_t i=0;i<NParams;i++) 
                newp->params(i)=gasdev()*eigensigmas(i);
            xdbg<<"steps = "<<newp->params<<endl;
            Assert(covar);
            newp->params = covar->svd().getU() * newp->params;
            xdbg<<"U*steps = "<<newp->params<<endl;
            for(size_t i=0;i<NParams;i++) 
                if (fixed_params[i]) newp->params[i] = 0.;
            xdbg<<"After adjust for fixed_params: "<<newp->params<<endl;
            newp->params += params;
            xdbg<<"+currentparams = "<<newp->params<<endl;
        } while (
            !newp->InRange() || 
            NegHsq(newp->GetOmM(),newp->omde(),newp->w(),newp->wa()));
        xdbg<<"done step"<<endl;
        return newp;
    }

    void operator+=(const MCPos& x)
    { params += x.params; }

    void operator-=(const MCPos& x)
    { params -= x.params; }

    void operator*=(const MCPos& x)
    { 
        for(size_t i=0;i<NParams;i++) params(i) *= x.params(i);
    }

    void operator/=(const MCPos& x)
    { 
        for(size_t i=0;i<NParams;i++) 
            if (x.params(i) != 0.0) params(i) /= x.params(i);
    }

    void operator*=(double x)
    { params *= x; }

    void operator/=(double x)
    { if (x != 0.0) params /= x; }

    void UpdateCovar(double lnlike, double eigenmult, double covmute)
    {
        // Call this using the position with the max likelihood found so far
        // to get the best results.
        // lnlike should be the lnlike value for this position

        if (!covar) {
            xdbg<<"start covarset\n";
            covar = new SymMatrix<double>(NParams,0.);
            for(size_t i=0;i<NParams;i++) 
                (*covar)(i,i) = initsigma.params(i)*initsigma.params(i);
            covar->divideUsing(tmv::SV);
            covar->setDiv();
            covar->saveDiv();
            for(size_t i = 0; i<NParams; i++) {
                eigensigmas(i) = sqrt(covar->svd().getS()(i));
                eigensigmas(i) = std::max(eigensigmas(i),MINESIGMA);
                eigensigmas(i) *= eigenmult;
                eigensigmas(i) = std::min(eigensigmas(i),MAXESIGMA);
                dbg<<"esigma("<<i<<") = "<<eigensigmas(i)<<endl;
            }
            dbg<<"done covarset\n";
        }

        dbg<<"start UpdateCovar"<<endl;
        // We set covar to be its inverse, Simga^-1, then we'll invert it later
        // First do the diagonal:
        dbg<<"current covar = "<<*covar<<endl;
        covar->divideUsing(tmv::LU);
        covar->resetDiv();
        *covar = covar->inverse();
        dbg<<"Inverted covar: "<<*covar<<endl;
        Vector<double> sigma(NParams);
        Vector<double> sigsq(NParams);
        for(size_t i=0;i<NParams;i++) {
            sigsq(i) = 1./(*covar)(i,i);
            sigma(i) = sqrt(sigsq(i));
        }
        dbg<<"Current best = "<<lnlike<<" at "<<params<<endl;

        vector<bool> docovar(NParams,false);
        for(size_t i=0;i<NParams;i++) if (includeincovarfit[i]) {
            docovar[i] = true;

            dbg<<"\nParameter "<<i<<": best value = "<<params(i)<<endl;
            double oldsigma;
            double oldsigsq;
            bool done = false;
            double xmin = params(i);
            double xmax = params(i);
            // Keep track of Ax=y eqn where each row of the equation is 
            // a + bx + cx^2 = chisq
            vector<vector<double> > A;
            vector<double> y;
            do {
                oldsigma = sigma(i);
                oldsigsq = sigsq(i);
                double delta = std::min(oldsigma,maxdelta.params(i));

                int k1 = -Ndelta;
                int k2 = Ndelta;
                int kmin=k1;
                double minchisq = 1.e100;
                for(int k=k1;k<=k2;k++) {
                    MCPos pos(1);
                    pos.params = params;
                    pos.params(i) += double(k)*delta;
                    double chisq = -2.*pos.CalcLnLike();
                    dbg<<"chisq("<<pos.params(i)<<") = "<<chisq<<endl;
                    if (chisq < minchisq) { minchisq = chisq; kmin = k; }
                    vector<double> Arow(3);
                    Arow[0] = 1.;
                    Arow[1] = pos.params(i);
                    Arow[2] = pos.params(i)*pos.params(i);
                    A.push_back(Arow);
                    y.push_back(chisq);
                    if (pos.params(i) > xmax) xmax = pos.params(i);
                    if (pos.params(i) < xmin) xmin = pos.params(i);
                }
                while (kmin == k1) {
                    dbg<<"Min chisq at kmin == -Ndelta: extend lower\n";
                    --k1;
                    MCPos pos(1);
                    pos.params = params;
                    pos.params(i) += double(k1)*delta;
                    double chisq = -2.*pos.CalcLnLike();
                    dbg<<"chisq("<<pos.params(i)<<") = "<<chisq<<endl;
                    if (chisq < minchisq) { minchisq = chisq; kmin = k1; }
                    vector<double> Arow(3);
                    Arow[0] = 1.;
                    Arow[1] = pos.params(i);
                    Arow[2] = pos.params(i)*pos.params(i);
                    A.push_back(Arow);
                    y.push_back(chisq);
                    if (pos.params(i) > xmax) xmax = pos.params(i);
                    if (pos.params(i) < xmin) xmin = pos.params(i);
                }
                while (kmin == k2) {
                    dbg<<"Min chisq at kmin == Ndelta: extend higher\n";
                    ++k2;
                    MCPos pos(1);
                    pos.params = params;
                    pos.params(i) += double(k2)*delta;
                    double chisq = -2.*pos.CalcLnLike();
                    dbg<<"chisq("<<pos.params(i)<<") = "<<chisq<<endl;
                    if (chisq < minchisq) { minchisq = chisq; kmin = k2; }
                    vector<double> Arow(3);
                    Arow[0] = 1.;
                    Arow[1] = pos.params(i);
                    Arow[2] = pos.params(i)*pos.params(i);
                    A.push_back(Arow);
                    y.push_back(chisq);
                    if (pos.params(i) > xmax) xmax = pos.params(i);
                    if (pos.params(i) < xmin) xmin = pos.params(i);
                }

                done = true;
                Matrix<double> AA(A);
                Vector<double> yy(y);
                AA.divideUsing(tmv::SV);
                Vector<double> xx = yy/AA;
                double invsigsq = xx(2);
                double center = -0.5*xx(1)/invsigsq;
                dbg<<"invsigsq = "<<invsigsq<<" (old value = "<<1./sigsq(i)<<")\n";
                dbg<<"center = "<<center<<" (old value = "<<params(i)<<")\n";
                double oldval = params(i);
                params(i) = center;
                if (!InRange()) {
                    dbg<<"new param is not in range, so skip this one.\n";
                    params(i) = oldval;
                    docovar[i] = false;
                } else {
                    MCPos pos(1);
                    pos.params = params;
                    double chisq = -2.*pos.CalcLnLike();
                    dbg<<"chisq("<<pos.params(i)<<") = "<<chisq<<endl;
                    if (chisq >= minchisq) {
                        dbg<<"supposed new value not better - keep old one.\n";
                        params(i) = oldval;
                        if (kmin != 0) params(i) += double(kmin)*delta;
                    }
                    if (invsigsq > 0.) {
                        (*covar)(i,i) = invsigsq;
                        sigsq(i) = 1./invsigsq;
                        sigma(i) = sqrt(sigsq(i));
                        dbg<<"sigma = "<<sigma(i)<<"  (old value = "<<oldsigma<<")\n";
                        double reldiff = abs(oldsigma-sigma(i))/sigma(i);
                        dbg<<"relative diff = "<<reldiff<<endl;
                        done = (reldiff < 0.1);
                    } else {
                        dbg<<"  new invsigsq = "<<invsigsq<<" !> 0, so skip this.\n";
                        docovar[i] = false;
                    }
                }
            } while (!done);
        } 
        // Now the off-diagonal using the new sigmas
        if (covmute) { // set covmute = 0 to skip covariance calculations
            dbg<<"Now do off-diagonal:"<<endl;
            for(size_t i=0;i<NParams;i++) if (includeincovarfit[i]) {
                for(size_t j=0;j<i;j++) if (includeincovarfit[j]) {

                    dbg<<"i,j = "<<i<<','<<j<<endl;
                    // Keep track of Ax=y eqn where each row of the equation is 
                    // a + bx + cy + dx^2 + exy + fy^2 = chisq
                    vector<vector<double> > A;
                    vector<double> y;

                    double deltai = std::min(sigma(i)/2.,maxdelta.params(i));
                    double deltaj = std::min(sigma(j)/2.,maxdelta.params(j));

                    dbg<<"chisq     ";
                    for(int l=-1;l<=1;l++) {
                        dbg<<params(j)+double(l)*deltaj<<"  ";
                    }
                    dbg<<endl;
                    for(int k=-1;k<=1;k++) {
                        dbg<<params(j)+double(k)*deltai<<"  ";
                        for(int l=-1;l<=1;l++) {
                            MCPos pos(1);
                            pos.params = params;
                            pos.params(i) += double(k)*deltai;
                            pos.params(j) += double(l)*deltaj;
                            double chisq = -2.*pos.CalcLnLike();
                            dbg<<chisq<<"  ";
                            vector<double> Arow(6);
                            Arow[0] = 1.;
                            Arow[1] = pos.params(i);
                            Arow[2] = pos.params(j);
                            Arow[3] = pos.params(i)*pos.params(i);
                            Arow[4] = pos.params(i)*pos.params(j);
                            Arow[5] = pos.params(j)*pos.params(j);
                            A.push_back(Arow);
                            y.push_back(chisq);
                        }
                        dbg<<endl;
                    }

                    Matrix<double> AA(A);
                    Vector<double> yy(y);
                    AA.divideUsing(tmv::SV);
                    Vector<double> xx = yy/AA;
                    double d = xx(3);
                    double e = xx(4);
                    double f = xx(5);
                    dbg<<"d = "<<d<<"  f = "<<f<<"  "<<"  e/2 = "<<e/2.<<endl;
                    dbg<<"d' = "<<(*covar)(i,i)<<"  f' = "<<(*covar)(j,j)<<endl;
                    dbg<<"Dimensionless version = "<<e/2./sqrt(d*f)<<endl;
                    dbg<<"Muted to = "<<covmute*e/2./sqrt(d*f)<<endl;
                    (*covar)(i,j) = (*covar)(j,i) = covmute*e/2.;
                }
            }
        }
        dbg<<"Done making covar: covar = "<<*covar<<endl;
        covar->divideUsing(tmv::LU);
        covar->resetDiv();
        *covar = covar->inverse();
        dbg<<"Inverse = "<<*covar<<endl;
        covar->divideUsing(tmv::SV);
        covar->setDiv();
        dbg<<"SVD: U = "<<covar->svd().getU()<<endl;
        dbg<<"SVD: S = "<<covar->svd().getS().diag()<<endl;
        for(size_t i = 0; i<NParams; i++) {
            eigensigmas(i) = sqrt(covar->svd().getS()(i));
            eigensigmas(i) = std::max(eigensigmas(i),MINESIGMA);
            eigensigmas(i) *= eigenmult;
            eigensigmas(i) = std::min(eigensigmas(i),MAXESIGMA);
        }
        dbg<<"eigensigmas = "<<eigensigmas<<endl;
    }

#ifdef SN
    double CalcLnLike() const
    {
        xdbg<<"Start SN Calclnlike"<<endl;
        double chisq;
        double Om_m = GetOmM();
        double Om_de = GetOmDE();
        compute_sn1a_chi2_(Om_m,Om_de,w(),wa(),chisq);
        xdbg<<"done: ll = "<<-chisq/2.0<<endl;
        return -chisq/2.0;
    }
#elif defined BO
    double CalcLnLike() const
    {
        xdbg<<"Start BO Calclnlike"<<endl;
        SetUpU();
        double da1089 = u->fk(u->w_z(1089.));
        double da035 = u->fk(u->w_z(0.35));
        double h035 = u->hsq(1/1.35);
        Assert(h035 > 0.);
        h035 = sqrt(h035);
        double dv035 = da035*da035*0.35/h035;
        dv035 = exp(log(dv035)/3.);
        double r = dv035 / da1089;
        xdbg<<"for baryons: dv(0.35) = "<<dv035<<", da(1089) = "<<da1089<<endl;
        xdbg<<"ratio = "<<r<<": compare to 0.0979 += 0.0036\n";
        double chisq = SQR(r-0.0979)/SQR(0.0036);
        xdbg<<"BO chisq = "<<chisq<<endl;
        return -chisq/2.;
    }
#else // LENS:
    double CalcLnLike() const
    {
        xdbg<<"Start Lens Calclnlike"<<endl;
        static bool first = true;
        static Vector<double>* r=0;
        static Vector<double>* data=0;
        static SymMatrix<double>* Sigma=0;
        static size_t nmap=0;
        static size_t ntot=0;

        if (NegHsq(GetOmM(),omde(),w(),wa())) return -1.e100;

        SetUpU();

        if (first) {
#ifdef DILLUT
            dbg<<"DILLUT = "<<DILLUT<<endl;
#endif
#ifdef BMODE
            dbg<<"BMODE = "<<BMODE<<endl;
#endif
            dbg<<"map range = "<<map_minr/ARCMIN<<" - "<<map_maxr/ARCMIN<<endl;
            dbg<<"var range = "<<var_minr/ARCMIN<<" - "<<var_maxr/ARCMIN<<endl;

            ReadData(r,data,Sigma,nmap);
            ntot = r->size();

            Sigma->divideUsing(tmv::SV);
            Sigma->setDiv();
            Sigma->saveDiv();
            Sigma->svd().thresh(singthresh);
            dbg<<"Sigma->getS = "<<Sigma->svd().getS().diag()<<std::endl;
            dbg<<"Sigma->getK = "<<Sigma->svd().getKMax()<<std::endl;

            first = false;
        }

        Vector<double> pred(ntot);
        for(int i=0;i<nmap;i++) {
            pred(i) = u->fmm((*r)(i));
            xdbg<<"map("<<(*r)(i)/ARCMIN<<") = "<<pred(i)<<"  "<<(*data)(i)<<endl;
        }
        for(int i=nmap;i<ntot;i++) {
            pred(i) = u->fvar((*r)(i));
            xdbg<<"var("<<(*r)(i)/ARCMIN<<") = "<<pred(i)<<"  "<<(*data)(i)<<endl;
        }
        Vector<double> diff = *data-pred;
        xdbg<<"diff = "<<diff<<std::endl;
        xdbg<<"Sigma = "<<*Sigma<<std::endl;
        xdbg<<"Sigma.inv = "<<Sigma->inverse()<<std::endl;
        xdbg<<"diff/(*Sigma) = "<<(diff/(*Sigma))<<std::endl;
        double chisq = diff * (diff/(*Sigma));
        xdbg<<"chisq = "<<chisq<<endl;

        xdbg<<"done: ll = "<<-chisq/2.0<<endl;
        if (chisq < 0.0) abort();
        return -chisq/2.0;
    }
#endif // SN

};

SymMatrix<double>* MCPos::covar = 0;
Vector<double> MCPos::eigensigmas(NParams,0.);
Universe* MCPos::u=0;
MCPos* MCPos::upos=0;

#ifdef DES
// om_bh2   lnomch2   h   n   lnAs   w   wa
MCPos initbest(0.0196,-2.10,0.7,1.0,-0.61,-1.0,0.0);
MCPos initsigma(0.001,0.1,0.03,0.077,0.1,0.1,0.3);
MCPos maxdelta(0.005,0.1,0.03,0.03,0.4,0.3,0.6);
#else
// The old setup.  Now commented out.
#ifdef NonFlat

#ifdef SN
#ifdef LAMBDA
MCPos initbest(0.023,-2.2,0.7,0.95,-0.4,0.73,-1.0,0.0);
MCPos initsigma(0.001,0.04,0.03,0.03,0.5,0.04,0.1,0.3);
#else
MCPos initbest(0.023,-2.2,0.7,0.95,-0.4,0.73,-1.0,0.0);
MCPos initsigma(0.001,0.04,0.03,0.03,0.5,0.04,0.3,0.3);
#endif // LAMBDA
#elif defined BO
#ifdef LAMBDA
MCPos initbest(0.02386,-1.649,0.612,1.098,-2.064,0.423,-1.0,0.0);
MCPos initsigma(0.001,0.059,0.03,0.077,0.13,0.15,0.1,0.3);
#else
MCPos initbest(0.02436,-2.580,0.602,1.330,-0.727,0.98,-1.19,-3.03);
MCPos initsigma(0.001,0.052,0.05,0.101,0.12,0.07,0.25,1.6);
#endif // LAMBDA
#else // LENS:
#ifdef LAMBDA
MCPos initbest(0.02386,-1.649,0.612,1.098,-2.064,0.423,-1.0,0.0);
MCPos initsigma(0.001,0.059,0.03,0.077,0.13,0.15,0.1,0.3);
#else
MCPos initbest(0.02436,-2.580,0.602,1.330,-0.727,0.98,-1.19,-3.03);
MCPos initsigma(0.001,0.052,0.05,0.101,0.12,0.07,0.25,1.6);
#endif // LAMBDA
#endif // SN
MCPos maxdelta(0.005,0.1,0.03,0.03,0.4,0.1,0.5,0.5);

#else // !NonFlat

#ifdef SN
#ifdef LAMBDA
MCPos initbest(0.023,-2.2,0.7,0.95,-0.4,-1.0,0.0);
MCPos initsigma(0.001,0.04,0.03,0.03,0.5,0.3,0.3);
#else
MCPos initbest(0.023,-2.2,0.7,0.95,-0.4,-1.0,0.0);
MCPos initsigma(0.001,0.04,0.03,0.03,0.5,0.3,0.3);
#endif // LAMBDA
#elif defined BO
#ifdef LAMBDA
MCPos initbest(0.02386,-1.649,0.612,1.098,-2.064,-1.0,0.0);
MCPos initsigma(0.001,0.059,0.03,0.077,0.13,0.1,0.3);
#else
MCPos initbest(0.02355,-1.899,0.887,0.95,-0.4,-2.0,0.0);
MCPos initsigma(0.024,0.159,0.06,0.03,0.3,0.37,0.3);
#endif // LAMBDA
#else // LENS:
#ifdef LAMBDA
MCPos initbest(0.02386,-1.649,0.612,1.098,-2.064,-1.0,0.0);
MCPos initsigma(0.001,0.059,0.03,0.077,0.13,0.1,0.3);
#else
MCPos initbest(0.02166,-1.696,0.614,1.081,-1.816,-0.675,0.0);
MCPos initsigma(0.001,0.049,0.03,0.077,0.12,0.14,0.3);
#endif // LAMBDA
#endif // SN
MCPos maxdelta(0.005,0.1,0.03,0.03,0.4,0.3,0.6);

#endif // NonFlat
#endif 

#ifndef SN
void ReadData(
    Vector<double>*& r, Vector<double>*& mean, SymMatrix<double>*& cov, 
    size_t& nmapout)
{
    dbg<<"Start ReadData\n";

    const size_t nfields = 12;
    const string datafile = "m2.";
    const char* fieldcodes[nfields] = { 
        "AA", "AB", "AC", "BA", "BB", "BC",
        "CA", "CB", "CC", "DA", "DB", "DC" };

    vector<vector<double> > rmap(nfields);
    vector<vector<double> > map(nfields);
    //vector<vector<double> > sigmap(nfields);
    vector<vector<double> > rvar(nfields);
    vector<vector<double> > var(nfields);
    //vector<vector<double> > sigvar(nfields);

    for(size_t k=0;k<nfields;k++) {
        string name = datafile + DATECODE + '.' + fieldcodes[k];
        ifstream fin(name.c_str());
        if (!fin) myerror("opening data file:  ",name.c_str());

        dbg<<"Reading file: "<<name<<endl;
        string line;
        for(int count=0; getline(fin,line); count++) {
            if (line[0] == '#') continue;
            if (count % datafrac != 0) continue;
            istringstream linein(line);
            double r,mapsq,mxsq,junk,gamsq;
            double sigmap,siggamsq;
            linein >>r>>mapsq>>sigmap>>mxsq>>junk>>junk>>junk>>junk>>junk>>gamsq>>siggamsq;
            r *= ARCMIN;

#ifdef BMODE
            mapsq += (BMODE)*mxsq;
#endif

#ifdef DILLUT
            mapsq *= DILLUTSQ;
            gamsq *= DILLUTSQ;
            //sigmap *= DILLUTSQ;
            //siggamsq *= DILLUTSQ;
#endif

            if (r > map_minr && r < map_maxr) {
                rmap[k].push_back(r);
                map[k].push_back(mapsq);
                //sigmap[k].push_back(sigmap);
            }
            if (r > var_minr && r < var_maxr) {
                rvar[k].push_back(r);
                var[k].push_back(gamsq);
                //sigvar[k].push_back(sigvar);
            }
        }
        dbg<<"sizes = "<<rmap[k].size()<<", "<<rvar[k].size()<<endl;
        if (k>0) {
            Assert(rmap[k].size() == rmap[0].size());
            Assert(rvar[k].size() == rvar[0].size());
        }
    }
    size_t nmap = rmap[0].size();
    size_t nvar = rvar[0].size();
    size_t ntot = nmap + nvar;
    dbg<<"nmap = "<<nmap<<", nvar = "<<nvar<<endl;

    r = new Vector<double>(ntot,0.);
    mean = new Vector<double>(ntot,0.);
    cov = new SymMatrix<double>(ntot,0.);

    for(size_t k=0;k<nfields;k++) {
        for(size_t i=0;i<nmap;i++) {
            (*r)[i] += rmap[k][i];
            (*mean)[i] += map[k][i];
            for(size_t j=0;j<=i;j++) {
                (*cov)(i,j) += map[k][i] * map[k][j];
            }
        }
        for(size_t i=0;i<nvar;i++) {
            (*r)[nmap+i] += rvar[k][i];
            (*mean)[nmap+i] += var[k][i];
            for(size_t j=0;j<=i;j++) {
                (*cov)(nmap+i,nmap+j) += var[k][i] * var[k][j];
            }
            for(size_t j=0;j<nmap;j++) {
                (*cov)(nmap+i,j) += var[k][i]*map[k][j];
            }
        }
    }

    *r /= double(nfields);
    *mean /= double(nfields);
    *cov -= double(nfields)*(*mean ^ *mean);
    *cov /= double(nfields*(nfields-1));
    for(size_t i=0;i<ntot;i++) Assert((*cov)(i,i) >= 0.);

#ifdef BMODE
    *cov /= sqrt(BMODE*BMODE+1);
#endif

    nmapout = nmap;

    if (XDEBUG) {
        SymMatrix<double> c2 = *cov;
        Vector<double> lambda(c2.size());
        Eigen(c2.view(),lambda.view());
        xdbg<<"Eigen values = "<<lambda<<std::endl;
    }
}
#endif

double CalcR(
    const vector<vector<MCPos*> >& chains, size_t i1, size_t i2,
    const vector<bool>& use)
{
    dbg<<"Start CalcR: "<<i1<<"--"<<i2<<endl;
    double M = count(use.begin(),use.end(),true);
    double N = i2-i1;
    static const MCPos zero(0.,0.,0.,0.,0.,0.,0.);
    dbg<<"M,N = "<<M<<","<<N<<endl;

    vector<MCPos> ybar(M,zero);
    MCPos totybar(zero);
    for(size_t j=0,jj=0;jj<chains.size();jj++) if(use[jj]) {
        for(size_t i=i1;i<i2;i++) ybar[j] += *chains[jj][i];
        ybar[j] /= double(N);
        totybar += ybar[j];
        dbg<<"ybar["<<j<<"] = "<<ybar[j].GetParams()<<endl;
        j++;
    }
    totybar /= double(M);
    dbg<<"totybar = "<<totybar.GetParams()<<endl;

    MCPos Bn(zero);
    for(size_t j=0;j<M;j++) {
        MCPos diff = ybar[j];
        diff -= totybar;
        diff *= diff;
        Bn += diff;
    }
    Bn /= double(M-1);

    MCPos W(zero);
    for(size_t i=i1;i<i2;i++) 
        for(size_t j=0,jj=0;jj<chains.size();jj++) if(use[jj]) {
            MCPos diff = *chains[jj][i];
            diff -= ybar[j];
            diff *= diff;
            W += diff;
            j++;
        }
    W /= double(M*(N-1));

    MCPos R = W;
    R *= double((N-1.)/N);
    Bn *= double(1.+1./M);
    R += Bn;
    R /= W;

    double maxR = 0.;
    dbg<<"R(ombh2) = "<<R.ombh2()<<endl;
    if (R.ombh2() > maxR) maxR = R.ombh2();
    dbg<<"R(lnomch2) = "<<R.lnomch2()<<endl;
    if (R.lnomch2() > maxR) maxR = R.lnomch2();
    dbg<<"R(n) = "<<R.n()<<endl;
    if (R.n() > maxR) maxR = R.n();
#ifndef LAMBDA
    dbg<<"R(w) = "<<R.w()<<endl;
    if (R.w() > maxR) maxR = R.w();
#endif
#ifdef Wa
    dbg<<"R(wa) = "<<R.wa()<<endl;
    if (R.wa() > maxR) maxR = R.wa();
#endif
    dbg<<"R(lnAs) = "<<R.lnAs()<<endl;
    if (R.lnAs() > maxR) maxR = R.lnAs();
    dbg<<"R(h) = "<<R.h()<<endl;
    if (R.h() > maxR) maxR = R.h();
    dbg<<"max = "<<maxR<<endl;

    return maxR;
}

double CalcR(const vector<vector<MCPos*> >& chains)
{
    dbg<<"Start CalcR"<<endl;
    dbg<<"chains.size = "<<chains.size()<<endl;
    size_t maxsize=0;
    for(size_t i=0;i<chains.size();i++) {
        if (chains[i].size() > maxsize) maxsize = chains[i].size();
        dbg<<"chains["<<i<<"].size = "<<chains[i].size()<<endl;
    }
    dbg<<"maxsize = "<<maxsize<<endl;
    if (maxsize == 0) return 100.;
    vector<bool> use(chains.size(),true);
    size_t minsize=maxsize;
    size_t nuse = chains.size();
    for(size_t i=0;i<chains.size();i++) {
        if (chains[i].size() < maxsize/10) {
            use[i] = false;
            dbg<<"Not using chaing # "<<i<<endl;
            nuse--;
        }
        else {
            if (chains[i].size() < minsize) minsize = chains[i].size();
        }
    }
    if (nuse <= 1) return 100.;
    dbg<<"minsize = "<<minsize<<endl;
    double R = CalcR(chains,minsize/2,minsize,use);
    dbg<<"R = "<<R<<endl;
    if (R < MAXR) {
        double R1 = CalcR(chains,0,minsize/2,use);
        double Rall = CalcR(chains,0,minsize,use);
        dbg<<"R1 = "<<R1<<", Rall = "<<Rall<<endl;
        if (R1 > R) R = R1;
        if (Rall > R) R = Rall;
    }
    return R;
}

int main(int argc, char* argv[]) 
{
    string name = NAME;
#ifdef NonFlat
    name += "_nf";
#endif
#ifdef LAMBDA
    name += "_flat";
#endif
    string debugname = string("predflat.") + name + ".debug";
    dbgout = new ofstream(debugname.c_str());
    //dbgout = &std::cout;
    string datname = string("lnlike.") + name + ".dat";
    ofstream fout(datname.c_str());

#ifdef Wa
    fout <<"#  om_m    om_lam     om_b        h        As       n         w       wa       sig8     datall      ll\n";
#else
    fout <<"#  om_m    om_lam     om_b        h        As       n         w       sig8     datall      ll\n";
#endif

    MCPos* bestpos=&initbest;
    dbg<<"initbest = "<<initbest.GetParams()<<endl;
    double bestlnlike=initbest.CalcLnLike();
    dbg<<"bestlnlike = "<<bestlnlike<<std::endl;
    dbg<<"Sigma8 = "<<initbest.Sigma8()<<std::endl;
    bestpos->UpdateCovar(bestlnlike,4.,0.);
    bestpos->ResetU();
    dbg<<"updated initbest = "<<initbest.GetParams()<<endl;
    bestlnlike = initbest.CalcLnLike();
    dbg<<"bestlnlike = "<<bestlnlike<<endl;

    vector<vector<MCPos*> > chains(NCHAIN);
    vector<MCPos*> burned;
    vector<MCPos*> pos(NCHAIN);
    vector<double> lnlike(NCHAIN);
    vector<double> sig8(NCHAIN);
    vector<int> mult(NCHAIN);
    dbg<<"Initial lnlike = ";
    //xdbg<<"Before initial loop: "<<endl;
    for(size_t j=0;j<NCHAIN;j++) {
        //xdbg<<"j = "<<j<<endl;
        pos[j] = new MCPos();
        //xdbg<<"pos[j] = "<<pos[j]->GetParams()<<endl;
        lnlike[j] = pos[j]->CalcLnLike();
        sig8[j] = pos[j]->Sigma8();
        mult[j] = 1;
        dbg<<lnlike[j]<<"  ";
        //xdbg<<"lnlike = "<<lnlike[j]<<endl;
    }
    dbg<<endl;
    for(size_t j=0;j<NCHAIN;j++) {
        xdbg<<"Initial params["<<j<<"] = "<<pos[j]->GetParams()<<endl;
        dbg<<"Sigma8["<<j<<"] = "<<sig8[j]<<std::endl;
    }

    int nextupdate = BURNIN/3;
    double emult = 2.;
    for(int count=0;;count++) {
        if (count % 100 == 0) {
            dbg<<"current n: ";
            for(size_t i=0;i<NCHAIN;i++) dbg<<chains[i].size()<<"  ";
            dbg<<endl;
        }
        xdbg<<"count = "<<count;
        xdbg<<endl;

        if (count == nextupdate) {
            dbg<<"\nbest params = "<<bestpos->GetParams()<<endl;
            bestpos->UpdateCovar(bestlnlike,emult,0.9);
            if (emult > 1.01) {
                nextupdate *= 2;
                emult /= 2.;
            } else {
                nextupdate = -1;
            }
            xdbg<<"and so,... \ncount = "<<count;
        }

        if (count%TESTFREQ == 0) {
            dbg<<endl;
            double R = CalcR(chains);
            dbg<<"\nR = "<<R<<endl;
            xdbg<<"count = "<<count;
            if (count>=MINPERCHAIN && R < MAXR) break;
        }

        for(size_t j=0;j<NCHAIN;j++) {
            xdbg<<"\n before make new step"<<endl;
            MCPos* newpos = pos[j]->Step();
            xdbg<<"\n after make new step"<<endl;

            double newlnlike = newpos->CalcLnLike();
            xdbg<<"newlnlike = "<<newlnlike;
            double lnratio = newlnlike-lnlike[j];
            xdbg<<"    lnratio = "<<lnratio<<endl;
            // ratio > ran0
            // ln(ratio) > ln(ran0)
            if (lnratio >= 0 || lnratio > log(ran0())) {
                if (newlnlike > bestlnlike) {
                    dbg<<"\nnewbest ("<<newlnlike<<") = "<<newpos->GetParams()<<endl;
                    if (newpos->w() < -0.4) {
                        // when w is close to 0 the derived covariance matrix 
                        // gets all screwy. Better to just ignore these.
                        bestpos = newpos;
                        bestlnlike = newlnlike;
                    } else {
                        dbg<<"(Not using since w is too high.)\n";
                    }
                    xdbg<<"count = "<<count;
                    for(size_t jj=0;jj<j;jj++) {
                        dbg<<"  "<<lnlike[jj];
                    }
                }

                xdbg<<"take step"<<endl;
                if (count > BURNIN) { // write old step first

                    chains[j].push_back(pos[j]);

                    double omm = pos[j]->GetOmM();
                    double lam = pos[j]->GetOmDE();
                    double omb = pos[j]->GetOmB();

                    fout <<omm<<"  "<<lam<<"  "<<omb<<"  ";
                    fout <<pos[j]->h()<<"  "<<pos[j]->As()<<"  ";
                    fout <<pos[j]->n()<<"  "<<pos[j]->w()<<"  ";
#ifdef Wa
                    fout <<pos[j]->wa()<<"  ";
#endif
                    fout <<sig8[j]<<"  0.  "<<log(double(mult[j]))<<endl;
                } else {
                    burned.push_back(pos[j]);
                }

                pos[j] = newpos;
                lnlike[j] = newlnlike;
                sig8[j] = newpos->Sigma8();
                mult[j] = 1;
                xdbg<<"done taking step"<<endl;
            } else {
                xdbg<<"don't take step"<<endl;
                mult[j]++;
                delete newpos;
            }
            dbg<<"  "<<lnlike[j];
        }
        dbg<<endl;
    }
    for(size_t j=0;j<NCHAIN;j++) for(size_t i=0;i<chains[j].size();i++) 
        delete chains[j][i];
    for(size_t i=0;i<burned.size();i++) 
        delete burned[i];

    return 0;
}
